// This file is part of MatrixPilot.
//
//    http://code.google.com/p/gentlenav/
//
// Copyright 2009, 2010 MatrixPilot Team
// See the AUTHORS.TXT file for a list of authors of MatrixPilot.
//
// MatrixPilot is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MatrixPilot is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MatrixPilot.  If not, see <http://www.gnu.org/licenses/>.


#include "defines.h"
//#include "../libUDB/libUDB_defines.h"

//	Perform control based on the airframe type.
//	Use the radio to determine the baseline pulse widths if the radio is on.
//	Otherwise, use the trim pulse width measured during power up.
//
//	Mix computed roll and pitch controls into the output channels for the compiled airframe type

#define ROLL_YAW				0.3
#define	AILERON_DIFFERENTIAL 	0.5
#define CAMBER_AILERON			0.1
#define CAMBER_FLAP				0.1
#define AIRBRAKE_FLAP			1
#define AIRBRAKE_AILERON		0.2
#define SPOILER_AILERON			0.5
#define PITCH_UP_CAMBER			0.05

const int rol_yaw 				= (ROLL_YAW * RMAX);
const int aileron_differential 	= (AILERON_DIFFERENTIAL * RMAX);
const int camber_flap			= (ROLL_YAW * RMAX);
const int airbrake_flap			= (AIRBRAKE_FLAP * RMAX);
const int airbrake_aileron		= (AIRBRAKE_AILERON * RMAX);

const int aileronbgain = (int)(8.0*AILERON_BOOST) ;
const int elevatorbgain = (int)(8.0*ELEVATOR_BOOST) ;
const int rudderbgain = (int)(8.0*RUDDER_BOOST) ;


void servoMix( void )
{
	//pwManual is the offset from trim value for a particular channel
	int pwManual[MAX_INPUTS+1];
	int pwOutput[MAX_OUTPUTS+1];

	union longww tempww;

	int temp;
	
	// If radio is off, use zero instead of the udb_pwIn-udbpwTrim values
	for (temp = 1; temp <= NUM_INPUTS; temp++)
		if (udb_flags._.radio_on)
			pwManual[temp] = udb_pwIn[temp] - udb_pwTrim[temp];
		else
			pwManual[temp] = 0;

	// Apply boosts if in a stabilized mode
/*	if (udb_flags._.radio_on && flags._.pitch_feedback)
	{
		pwManual[AILERON_INPUT_CHANNEL] += ((pwManual[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]) * aileronbgain) >> 3 ;
		pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3 ;
		pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3 ;
	}
*/
	
	pwOutput[AILERON_LEFT_OUTPUT_CHANNEL] 	= pwManual[AILERON_LEFT_INPUT_CHANNEL];
	pwOutput[AILERON_RIGHT_OUTPUT_CHANNEL] 	= pwManual[AILERON_RIGHT_INPUT_CHANNEL];

	pwOutput[FLAP_LEFT_OUTPUT_CHANNEL] 		= pwManual[FLAP_LEFT_INPUT_CHANNEL];
	pwOutput[FLAP_RIGHT_OUTPUT_CHANNEL] 	= pwManual[FLAP_RIGHT_INPUT_CHANNEL];

	pwOutput[ELEVATOR_OUTPUT_CHANNEL] 		= pwManual[ELEVATOR_INPUT_CHANNEL];
	pwOutput[RUDDER_OUTPUT_CHANNEL] 		= pwManual[RUDDER_INPUT_CHANNEL];

	pwOutput[THROTTLE_OUTPUT_CHANNEL] 		= pwManual[THROTTLE_INPUT_CHANNEL];

	temp = REVERSE_IF_NEEDED(ROLL_CONTROL_REVERSED, roll_control + waggle);
	tempww.WW = __builtin_mulss(temp << 2, aileron_differential);

	if(temp >= 0)
	{
		pwOutput[AILERON_LEFT_OUTPUT_CHANNEL] 	+= REVERSE_IF_NEEDED(AILERON_LEFT_CHANNEL_REVERSED, temp);
		pwOutput[AILERON_RIGHT_OUTPUT_CHANNEL] 	-= REVERSE_IF_NEEDED(AILERON_RIGHT_CHANNEL_REVERSED, tempww._.W1);
	}
	else
	{
		pwOutput[AILERON_LEFT_OUTPUT_CHANNEL] 	+= REVERSE_IF_NEEDED(AILERON_LEFT_CHANNEL_REVERSED, tempww._.W1);
		pwOutput[AILERON_RIGHT_OUTPUT_CHANNEL]	-= REVERSE_IF_NEEDED(AILERON_RIGHT_CHANNEL_REVERSED, temp);
	}

//	pwOutput[AILERON_OUTPUT_CHANNEL]		-= ( airbrake-aileron 	* ( brake_control << 1) );
//	pwOutput[AILERON2_OUTPUT_CHANNEL]		-= ( airbrake-aileron 	* ( brake_control << 1) );
//	pwOutput[FLAP_OUTPUT_CHANNEL]			+= ( airbrake-flap 		* ( brake_control << 1) );
//	pwOutput[FLAP2_OUTPUT_CHANNEL]			+= ( airbrake-flap 		* ( brake_control << 1) );

	pwOutput[RUDDER_OUTPUT_CHANNEL] 	+= REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control + waggle);
	pwOutput[ELEVATOR_OUTPUT_CHANNEL] 	+= REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control + waggle);

	udb_pwOut[AILERON_LEFT_OUTPUT_CHANNEL] 	= udb_servo_pulsesat( pwOutput[AILERON_LEFT_OUTPUT_CHANNEL] 	+ udb_pwTrim[AILERON_LEFT_INPUT_CHANNEL] ) ;
	udb_pwOut[AILERON_RIGHT_OUTPUT_CHANNEL]	= udb_servo_pulsesat( pwOutput[AILERON_RIGHT_OUTPUT_CHANNEL] 	+ udb_pwTrim[AILERON_RIGHT_INPUT_CHANNEL]  ) ;
	udb_pwOut[FLAP_LEFT_OUTPUT_CHANNEL] 	= udb_servo_pulsesat( pwOutput[FLAP_LEFT_OUTPUT_CHANNEL] 		+ udb_pwTrim[FLAP_LEFT_INPUT_CHANNEL] ) ;
	udb_pwOut[FLAP_RIGHT_OUTPUT_CHANNEL]	= udb_servo_pulsesat( pwOutput[FLAP_RIGHT_OUTPUT_CHANNEL] 		+ udb_pwTrim[FLAP_RIGHT_INPUT_CHANNEL] ) ;
	udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] 		= udb_servo_pulsesat( pwOutput[ELEVATOR_OUTPUT_CHANNEL]			+ udb_pwTrim[ELEVATOR_INPUT_CHANNEL] ) ;
	udb_pwOut[RUDDER_OUTPUT_CHANNEL] 		= udb_servo_pulsesat( pwOutput[RUDDER_OUTPUT_CHANNEL] 			+ udb_pwTrim[RUDDER_INPUT_CHANNEL] ) ;


	if ( pwOutput[THROTTLE_INPUT_CHANNEL] < 0 )
	{
		pwOutput[THROTTLE_OUTPUT_CHANNEL] = udb_pwTrim[THROTTLE_INPUT_CHANNEL] ;
	}
	else
	{	
		temp = pwManual[THROTTLE_INPUT_CHANNEL] + REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, altitude_control) + udb_pwTrim[THROTTLE_INPUT_CHANNEL];
		udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = udb_servo_pulsesat( temp ) ;
	}
	

}
